% Graph-SLAM仿真主程序（激光雷达版）

clear all;
close all;
clc;

% 添加路径
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'common'));
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'occupancy_grid'));

% 轨迹选择
fprintf('========================================\n');
fprintf('  Graph-SLAM 轨迹模式选择\n');
fprintf('========================================\n');
fprintf('1. 固定路径点轨迹（可重复，适合对比）\n');
fprintf('2. 动态避障轨迹（随机探索）\n');
fprintf('========================================\n');
choice = input('请选择轨迹模式 [1/2] (默认1): ', 's');
if isempty(choice)
    choice = '1';
end

%% 加载统一配置
config = get_slam_config();

% 应用选择
if choice == '1'
    config.use_fixed_trajectory = true;
    fprintf('使用固定路径点轨迹\n\n');
elseif choice == '2'
    config.use_fixed_trajectory = false;
    fprintf('使用动态避障轨迹\n\n');
else
    fprintf('无效选择，使用默认（固定路径点）\n\n');
    config.use_fixed_trajectory = true;
end

% 解包常用参数
dt = config.dt;
total_time = config.total_time;
steps = config.steps;
Q = config.Q;
R = config.R;
laser_range = config.laser_range;
laser_fov = config.laser_fov;
num_beams = config.num_beams;
laser_noise = config.laser_noise;
map_size = config.map_size;

%% 读取地图和障碍物信息
fprintf('=== 加载环境地图 ===\n');
[landmarks, obstacles] = generate_landmarks();
n_obstacles = size(obstacles, 1);

fprintf('地图尺寸: %.0f x %.0f m\n', map_size(1), map_size(2));
fprintf('障碍物总数: %d\n', n_obstacles);

% 分析障碍物布局
fprintf('\n障碍物布局:\n');
fprintf('  - 外墙: 4个 (边界保护)\n');
fprintf('  - 内部障碍物: %d个\n', n_obstacles - 4);
for i = 5:n_obstacles  % 跳过外墙
    obs = obstacles(i, :);
    fprintf('    [%.0f,%.0f] to [%.0f,%.0f]\n', obs(1), obs(2), obs(3), obs(4));
end

fprintf('\n传感器配置:\n');
fprintf('  激光雷达范围: %.0f m\n', laser_range);
fprintf('  激光雷达视野: %.0f°\n', rad2deg(laser_fov));
fprintf('  激光束数量: %d\n', num_beams);

%% 初始化状态
if config.use_fixed_trajectory
    true_state = config.initial_state;
else
    % 动态模式：使用随机初始方向
    initial_theta = 0;  % 固定初始方向：水平向右
    true_state = [config.initial_state(1); config.initial_state(2); initial_theta];
end
fprintf('\n初始位置: (%.1f, %.1f), 朝向: %.2f rad\n', true_state(1), true_state(2), true_state(3));

%% 轨迹规划
% 使用统一配置
trajectory_mode = config.trajectory_type;
USE_FIXED_TRAJECTORY = config.use_fixed_trajectory;

fprintf('\n=== 轨迹规划模式: %s ===\n', trajectory_mode);

if USE_FIXED_TRAJECTORY
    switch trajectory_mode
        case 'waypoint'
            fprintf('使用固定路径点轨迹\n');
            controls = generate_waypoint_trajectory(config.waypoints, dt, total_time, true_state);
            
        case 'smart'
            % 使用智能轨迹规划
            controls = generate_smart_trajectory(dt, total_time, obstacles, true_state, map_size);
            
        case 'safe_exploration'
            % 使用预定义的安全探索轨迹
            fprintf('使用预定义安全探索轨迹\n');
            controls = generate_fixed_trajectory(dt, total_time, 'safe_exploration', obstacles, true_state);
            
        otherwise
            % 其他固定轨迹类型
            controls = generate_fixed_trajectory(dt, total_time, trajectory_mode, obstacles, true_state);
    end
else
    % 使用随机轨迹+实时避障
    fprintf('使用随机轨迹模式（带实时避障）\n');
    controls = [];
    v_base = config.v_base;
    omega_base = config.omega_base;
end

%% 轨迹预览
if USE_FIXED_TRAJECTORY
    fprintf('\n生成轨迹预览...\n');
    
    % 如果是waypoint模式，传递路径点进行可视化
    if strcmp(trajectory_mode, 'waypoint')
        visualize_map_and_trajectory(obstacles, true_state, controls, dt, map_size, config.waypoints);
    else
        visualize_map_and_trajectory(obstacles, true_state, controls, dt, map_size);
    end
    
    fprintf('\n按任意键开始Graph-SLAM仿真...\n');
    pause;
end

%% 数据存储
true_trajectory = zeros(3, steps);
pose_graph = struct();
pose_graph.poses = [];
pose_graph.edges = [];
pose_graph.landmark_map = containers.Map('KeyType', 'int32', 'ValueType', 'any');

% 运动控制状态（仅在随机轨迹模式下使用）
if ~USE_FIXED_TRAJECTORY
    motion_state = struct();
    motion_state.boundary_avoiding = false;
    motion_state.obstacle_avoiding = false;
    motion_state.avoidance_timer = 0;
    motion_state.angle_diff = 0;
    motion_state.v = config.v_base;
    motion_state.omega = config.omega_base;
end

%% 阶段1: 数据收集
fprintf('阶段1: 数据收集...\n');
figure('Position', config.display.figure_position);

for t = 1:steps
    %% 获取控制输入
    if USE_FIXED_TRAJECTORY
        control = controls(:, t);
    else
        [v, omega, motion_state] = safe_motion_controller(true_state, obstacles, motion_state, v_base, omega_base, dt);
        control = [v; omega];
    end
    
    %% 真实运动
    true_state = motion_model(true_state, control, dt, Q);
    
    % 安全检查
    [still_collision, ~] = check_collision(true_state, obstacles, 0.5);
    if still_collision && t > 1
        true_state = true_trajectory(:, t-1);
    end
    
    true_trajectory(:, t) = true_state;
    pose_graph.poses = [pose_graph.poses, true_state];
    
    %% 添加里程计边
    if t > 1
        edge = struct();
        edge.type = 'odometry';
        edge.from = t - 1;
        edge.to = t;
        edge.pose_id = 0;
        edge.landmark_id = 0;
        edge.measurement = control;
        edge.dt = dt;
        edge.info = inv(Q(1:2, 1:2));
        pose_graph.edges = [pose_graph.edges; edge];
    end
    
    %% 激光观测
    [observations, ~] = observation_model_laser(true_state, obstacles, ...
                                    laser_range, laser_fov, num_beams, laser_noise);
    
    %% 数据关联与添加观测边
    if ~isempty(observations)
        % 提取已知路标信息（从图中估计）
        landmarks = [];
        landmark_ids = [];
        
        if pose_graph.landmark_map.Count > 0
            keys = pose_graph.landmark_map.keys();
            for k = 1:length(keys)
                lm_id = keys{k};
                landmark_ids = [landmark_ids; lm_id];
                
                % 从相关边中估计路标位置（使用最近的观测）
                lm_idx = pose_graph.landmark_map(lm_id);
                lm_pos = estimate_landmark_position(pose_graph, lm_idx, t);
                landmarks = [landmarks; lm_pos'];
            end
        end
        
        % 数据关联（使用自适应阈值）
        [associations, is_new] = data_association(observations, true_state, ...
                                                  landmarks, landmark_ids, [], R, []);
        
        % 添加观测边
        for i = 1:size(observations, 2)
            z = observations(:, i);
            
            if is_new(i)
                % 新路标
                new_id = pose_graph.landmark_map.Count + 1;
                pose_graph.landmark_map(new_id) = new_id;
                lm_idx = new_id;
            else
                % 已知路标
                assoc_idx = associations(i);
                lm_id = landmark_ids(assoc_idx);
                lm_idx = pose_graph.landmark_map(lm_id);
            end
            
            edge = struct();
            edge.type = 'landmark';
            edge.from = 0;
            edge.to = 0;
            edge.pose_id = t;
            edge.landmark_id = lm_idx;
            edge.measurement = z;
            edge.dt = 0;
            edge.info = inv(R);
            pose_graph.edges = [pose_graph.edges; edge];
        end
    end
    
    %% 可视化
    if mod(t, config.display.update_interval * 2) == 0  % Graph-SLAM使用较低频率
        plot_graph_slam(true_state, pose_graph, true_trajectory(:, 1:t), obstacles, t*dt);
        drawnow;
    end
end

%% 阶段2: 图优化
fprintf('\n阶段2: 图优化...\n');
n_poses = size(pose_graph.poses, 2);
n_landmarks = pose_graph.landmark_map.Count;
fprintf('图优化: %d个位姿, %d个路标\n', n_poses, n_landmarks);

optimized_graph = optimize_pose_graph(pose_graph, config.graph_slam.n_iterations);

%% 可视化结果
figure('Position', [100, 100, 1400, 700]);
plot_graph_slam_result(true_trajectory, optimized_graph.poses, landmarks, optimized_graph.landmark_map);

%% 结果统计
fprintf('\n=== 优化完成 ===\n');
fprintf('轨迹模式: %s\n', trajectory_mode);
fprintf('位姿节点数: %d\n', n_poses);
fprintf('发现特征: %d\n', n_landmarks);

position_error = sqrt(sum((true_trajectory(1:2, :) - optimized_graph.poses(1:2, :)).^2, 1));
fprintf('平均定位误差: %.3f m\n', mean(position_error));
fprintf('最大定位误差: %.3f m\n', max(position_error));

% 检查是否有碰撞
collision_count = 0;
for t = 1:size(true_trajectory, 2)
    [collision, ~] = check_collision(true_trajectory(:, t), obstacles, 0.5);
    if collision
        collision_count = collision_count + 1;
    end
end
fprintf('碰撞次数: %d / %d (%.1f%%)\n', collision_count, size(true_trajectory, 2), ...
    100 * collision_count / size(true_trajectory, 2));

% 计算路径覆盖范围
x_range = max(true_trajectory(1, :)) - min(true_trajectory(1, :));
y_range = max(true_trajectory(2, :)) - min(true_trajectory(2, :));
fprintf('探索范围: %.1f x %.1f m\n', x_range, y_range);

save('graph_slam_result.mat', 'true_trajectory', 'optimized_graph', 'position_error', ...
    'obstacles', 'trajectory_mode');
